#include<memory>
#include"rclcpp/rclcpp.hpp"
#include"nav2_msgs/action/navigate_to_pose.hpp"
#include"rclcpp_action/rclcpp_action.hpp"

using namespace std;

class NavToPoseClient: public rclcpp::Node
{
private:
    rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr action_client;
    nav2_msgs::action::NavigateToPose::Goal goal_pose;
    rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions send_goal_options;

public:
    NavToPoseClient(const string& node_name):Node(node_name)
    {
        action_client = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,"navigate_to_pose");
    }
    void SendGoalPose()
    {
        while(action_client->wait_for_action_server(chrono::seconds(5)) == false) RCLCPP_INFO(get_logger(),"等待action服务上线");
        goal_pose.pose.header.frame_id = "map";
        goal_pose.pose.pose.position.x = 2.0;
        goal_pose.pose.pose.position.y = 2.0;
        goal_pose.pose.pose.orientation.w = 1.0;
        //设置目标请求结果回调函数
        send_goal_options.goal_response_callback = 
            [this](rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr goal_handle)
            {
                if(goal_handle) RCLCPP_INFO(get_logger(),"目标点已被服务器接收");
            };
        //设置移动过程反馈回调函数
        send_goal_options.feedback_callback = 
            [this](rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr goal_handle,
                const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback)
            {
                (void)goal_handle;
                RCLCPP_INFO(get_logger(),"反馈剩余距离:%f",feedback->distance_remaining);
            };
        //设置执行结果回调函数
        send_goal_options.result_callback = 
            [this](const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult& result)
            {
                if(result.code == rclcpp_action::ResultCode::SUCCEEDED) RCLCPP_INFO(get_logger(),"处理成功!");
            };
        //发送目标导航点
        action_client->async_send_goal(goal_pose,send_goal_options);
    }

};

int main(int argc,char** argv)
{
    rclcpp::init(argc,argv);
    shared_ptr<NavToPoseClient> node = make_shared<NavToPoseClient>("nav_to_pose_client");
    node->SendGoalPose();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}